
#include "PoseEstimator.hpp"

PoseEstimator::PoseEstimator()
{
	
	fcount = 0;
	Bv.clear();

	frame_num = 3;

}



PoseEstimator::~PoseEstimator()
{

	

}

int PoseEstimator::Solve()
{
	int ret = 1;

	// get B from Bv
	// B = Mat(Bv);  // RISK RISK
	Mat tmp;
	B = Mat::zeros(6, Bv.size(), CV_64FC1);
	for (int i=0; i<Bv.size(); i++)
	{
		tmp = Bv[i];
		tmp.copyTo(B.col(i));
	}

	printf("Matrix size (%d, %d)\n", B.rows, B.cols);
	MATmtxdisp(B);
	getchar();

	// log to file
	ofstream LoggerIn("B.txt", ios_base::app);
	LoggerIn << B << endl;

	// solve the homogeneous linear equation

	return ret;
}

int PoseEstimator::Calc_vij(const Mat hi, const Mat hj, Mat& vij)
{
	int ret = 1;
	double hi1, hi2, hi3, hj1, hj2, hj3;

	vij = Mat::zeros(6, 1, CV_64FC1);

	hi1 = hi.at<double>(0);
	hi2 = hi.at<double>(1);
	hi3 = hi.at<double>(2);
	hj1 = hj.at<double>(0);
	hj2 = hj.at<double>(1);
	hj3 = hj.at<double>(2);

	vij.at<double>(0) = hi1*hj1;
	vij.at<double>(1) = hi1*hj2 + hi2*hj1;
	vij.at<double>(2) = hi2*hj2;
	vij.at<double>(3) = hi3*hj1 + hi1*hj3;
	vij.at<double>(4) = hi3*hj2 + hi2*hj3;
	vij.at<double>(5) = hi3*hi3;

	return ret;
}


int PoseEstimator::Estimate(const Mat H, Mat& M)
{
	Mat h1, h2, h3;
	double hi1, hi2, hi3, hj1, hj2, hj3;
	Mat v12, v11, v22;

	int ret = 1;

	if (fcount < frame_num)
	{

		fcount++;

		H.col(0).copyTo(h1);  // RISK RISK RISK
		H.col(1).copyTo(h2);
		H.col(2).copyTo(h3);

		Calc_vij(h1, h2, v12);
		Calc_vij(h1, h1, v11);
		Calc_vij(h2, h2, v22);

		Bv.push_back(v12);
		Bv.push_back(v11-v22);

	} else
	{
		Solve();
	}

	return ret;
}